// Transform.cpp: implementation of the CTransform class.
//
//////////////////////////////////////////////////////////////////////
#include "StdAfx.h"
#include "Transform.h"
#include "math.h"
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CTransform::CTransform()
{
	makeIdentity();
}

CTransform::~CTransform()
{

}

CTransform::CTransform( const CTransform & mat )
{
	set(mat.ptr());
}

void CTransform::Serialize(CArchive & ar)
{
	if(ar.IsStoring())
	{
		ar<<m_Data[0][0];
		ar<<m_Data[0][1];
		ar<<m_Data[0][2];
		ar<<m_Data[0][3];
		ar<<m_Data[1][0];
		ar<<m_Data[1][1];
		ar<<m_Data[1][2];
		ar<<m_Data[1][3];
		ar<<m_Data[2][0];
		ar<<m_Data[2][1];
		ar<<m_Data[2][2];
		ar<<m_Data[2][3];
		ar<<m_Data[3][0];
		ar<<m_Data[3][1];
		ar<<m_Data[3][2];
		ar<<m_Data[3][3];
	}
	else
	{
		ar>>m_Data[0][0];
		ar>>m_Data[0][1];
		ar>>m_Data[0][2];
		ar>>m_Data[0][3];
		ar>>m_Data[1][0];
		ar>>m_Data[1][1];
		ar>>m_Data[1][2];
		ar>>m_Data[1][3];
		ar>>m_Data[2][0];
		ar>>m_Data[2][1];
		ar>>m_Data[2][2];
		ar>>m_Data[2][3];
		ar>>m_Data[3][0];
		ar>>m_Data[3][1];
		ar>>m_Data[3][2];
		ar>>m_Data[3][3];
	}
}

CTransform& CTransform::operator = (const CTransform & rhs)
{
	if( &rhs == this ) 
		return *this;

	set(rhs.ptr());
	return *this;
}

void CTransform::set(const CTransform & rhs)
{
	set(rhs.ptr());
}

#define SET_ROW(m_iRow, v1, v2, v3, v4 )    \
	m_Data[(m_iRow)][0] = (v1); \
	m_Data[(m_iRow)][1] = (v2); \
	m_Data[(m_iRow)][2] = (v3); \
	m_Data[(m_iRow)][3] = (v4);

#define INNER_PRODUCT(a,b,r,c) \
	((a).m_Data[r][0] * (b).m_Data[0][c]) \
	+((a).m_Data[r][1] * (b).m_Data[1][c]) \
	+((a).m_Data[r][2] * (b).m_Data[2][c]) \
	+((a).m_Data[r][3] * (b).m_Data[3][c])


CTransform::CTransform( double a00, double a01, double a02, double a03,
											 double a10, double a11, double a12, double a13,
											 double a20, double a21, double a22, double a23,
											 double a30, double a31, double a32, double a33)
{
	SET_ROW(0, a00, a01, a02, a03 )
	SET_ROW(1, a10, a11, a12, a13 )
	SET_ROW(2, a20, a21, a22, a23 )
	SET_ROW(3, a30, a31, a32, a33 )
}

void CTransform::set( double a00, double a01, double a02, double a03,
								double a10, double a11, double a12, double a13,
								double a20, double a21, double a22, double a23,
								double a30, double a31, double a32, double a33)
{
	SET_ROW(0, a00, a01, a02, a03 )
	SET_ROW(1, a10, a11, a12, a13 )
	SET_ROW(2, a20, a21, a22, a23 )
	SET_ROW(3, a30, a31, a32, a33 )
}

#define QX  q._v[0]
#define QY  q._v[1]
#define QZ  q._v[2]
#define QW  q._v[3]

void CTransform::setTrans( double tx, double ty, double tz )
{
	m_Data[3][0] = tx;
	m_Data[3][1] = ty;
	m_Data[3][2] = tz;
}

void CTransform::setTrans( const CVec & v )
{
	m_Data[3][0] = v[0];
	m_Data[3][1] = v[1];
	m_Data[3][2] = v[2];
}

void CTransform::makeIdentity()
{
	SET_ROW(0,    1, 0, 0, 0 )
	SET_ROW(1,    0, 1, 0, 0 )
	SET_ROW(2,    0, 0, 1, 0 )
	SET_ROW(3,    0, 0, 0, 1 )
}

void CTransform::makeScale( const CVec & v )
{
	makeScale(v[0], v[1], v[2] );
}

void CTransform::makeScale( double x, double y, double z )
{
	SET_ROW(0,    x, 0, 0, 0 )
	SET_ROW(1,    0, y, 0, 0 )
	SET_ROW(2,    0, 0, z, 0 )
	SET_ROW(3,    0, 0, 0, 1 )
}

void CTransform::makeTranslate( const CVec & v )
{
	makeTranslate( v[0], v[1], v[2] );
}

void CTransform::makeTranslate( double x, double y, double z )
{
	SET_ROW(0,    1, 0, 0, 0 )
	SET_ROW(1,    0, 1, 0, 0 )
	SET_ROW(2,    0, 0, 1, 0 )
	SET_ROW(3,    x, y, z, 1 )
}

void CTransform::makeRotate( const CVec & from, const CVec & to, const CVec *pRefVec)
{
	makeIdentity();

	CVec norm=from.CrossProduct(to);
	double ang=from.AngleTo(to, &norm);
	
	if(fabs(ang)<0.00001)
		return;
	if(fabs(ang-3.1415926)<0.00001)
	{
		if(pRefVec)
			norm=*pRefVec;
		else
		{
			if(!from.IsParallel(CVec(1, 0, 0), 0.01))
				norm=from.CrossProduct(CVec(1, 0, 0));
			else
				norm=from.CrossProduct(CVec(0, 1, 0));
		}
	}

	makeRotate(ang, norm.GetVecX(), norm.GetVecY(), norm.GetVecZ());
}

void CTransform::makeRotate( double angle, const CVec & axis )
{
	makeRotate(angle, axis.GetVecX(), axis.GetVecY(), axis.GetVecZ());
}

void CTransform::makeRotate( double ang, double x, double y, double z ) 
{
	makeIdentity();

	double dblLength=sqrt(x*x+y*y+z*z);
	x=x/dblLength;
	y=y/dblLength;
	z=z/dblLength;

	m_Data[0][0]=cos(ang)+(1-cos(ang))*x*x;
	m_Data[0][1]=(1-cos(ang))*y*x+sin(ang)*z;
	m_Data[0][2]=(1-cos(ang))*z*x-sin(ang)*y;

	m_Data[1][0]=(1-cos(ang))*x*y-sin(ang)*z;
	m_Data[1][1]=cos(ang)+(1-cos(ang))*y*y;
	m_Data[1][2]=(1-cos(ang))*z*y+sin(ang)*x;

	m_Data[2][0]=(1-cos(ang))*x*z+sin(ang)*y;
	m_Data[2][1]=(1-cos(ang))*y*z-sin(ang)*x;
	m_Data[2][2]=cos(ang)+(1-cos(ang))*z*z;
}

void CTransform::makeMirrorByPlane(double nx, double ny, double nz)
{
	makeIdentity();

	m_Data[0][0]=1-2*nx*nx;
	m_Data[1][0]=-2*nx*ny;
	m_Data[2][0]=-2*nx*nz;

	m_Data[0][1]=-2*nx*ny;
	m_Data[1][1]=1-2*ny*ny;
	m_Data[2][1]=-2*ny*nz;

	m_Data[0][2]=-2*nx*nz;
	m_Data[1][2]=-2*ny*nz;
	m_Data[2][2]=1-2*nz*nz;
	//m_Data[0][0]=1-2*nx*nx;
	//m_Data[0][1]=-2*nx*ny;
	//m_Data[0][2]=-2*nx*nz;

	//m_Data[1][0]=-2*nx*ny;
	//m_Data[1][1]=1-2*ny*ny;
	//m_Data[1][2]=-2*ny*nz;

	//m_Data[2][0]=-2*nx*nz;
	//m_Data[2][1]=-2*ny*nz;
	//m_Data[2][2]=1-2*nz*nz;
}

void CTransform::mult( const CTransform& lhs, const CTransform& rhs )
{   
	if (&lhs==this)
	{
		postMult(rhs);
		return;
	}
	if (&rhs==this)
	{
		preMult(lhs);
		return;
	}

	// PRECONDITION: We assume neither &lhs nor &rhs == this
	// if it did, use preMult or postMult instead
	m_Data[0][0] = INNER_PRODUCT(lhs, rhs, 0, 0);
	m_Data[0][1] = INNER_PRODUCT(lhs, rhs, 0, 1);
	m_Data[0][2] = INNER_PRODUCT(lhs, rhs, 0, 2);
	m_Data[0][3] = INNER_PRODUCT(lhs, rhs, 0, 3);
	m_Data[1][0] = INNER_PRODUCT(lhs, rhs, 1, 0);
	m_Data[1][1] = INNER_PRODUCT(lhs, rhs, 1, 1);
	m_Data[1][2] = INNER_PRODUCT(lhs, rhs, 1, 2);
	m_Data[1][3] = INNER_PRODUCT(lhs, rhs, 1, 3);
	m_Data[2][0] = INNER_PRODUCT(lhs, rhs, 2, 0);
	m_Data[2][1] = INNER_PRODUCT(lhs, rhs, 2, 1);
	m_Data[2][2] = INNER_PRODUCT(lhs, rhs, 2, 2);
	m_Data[2][3] = INNER_PRODUCT(lhs, rhs, 2, 3);
	m_Data[3][0] = INNER_PRODUCT(lhs, rhs, 3, 0);
	m_Data[3][1] = INNER_PRODUCT(lhs, rhs, 3, 1);
	m_Data[3][2] = INNER_PRODUCT(lhs, rhs, 3, 2);
	m_Data[3][3] = INNER_PRODUCT(lhs, rhs, 3, 3);
}

void CTransform::preMult( const CTransform& other )
{
	// brute force method requiring a copy
	//CTransform tmp(other* *this);
	// *this = tmp;

	// more efficient method just use a double[4] for temporary storage.
	double t[4];
	for(int col=0; col<4; ++col) {
		t[0] = INNER_PRODUCT( other, *this, 0, col );
		t[1] = INNER_PRODUCT( other, *this, 1, col );
		t[2] = INNER_PRODUCT( other, *this, 2, col );
		t[3] = INNER_PRODUCT( other, *this, 3, col );
		m_Data[0][col] = t[0];
		m_Data[1][col] = t[1];
		m_Data[2][col] = t[2];
		m_Data[3][col] = t[3];
	}

}

void CTransform::postMult( const CTransform& other )
{
	// brute force method requiring a copy
	//CTransform tmp(*this * other);
	// *this = tmp;

	// more efficient method just use a double[4] for temporary storage.
	double t[4];
	for(int row=0; row<4; ++row)
	{
		t[0] = INNER_PRODUCT( *this, other, row, 0 );
		t[1] = INNER_PRODUCT( *this, other, row, 1 );
		t[2] = INNER_PRODUCT( *this, other, row, 2 );
		t[3] = INNER_PRODUCT( *this, other, row, 3 );
		SET_ROW(row, t[0], t[1], t[2], t[3] )
	}
}

#undef INNER_PRODUCT

/******************************************
Matrix inversion technique:
Given a matrix mat, we want to invert it.
mat = [ r00 r01 r02 a
r10 r11 r12 b
r20 r21 r22 c
tx  ty  tz  d ]
We note that this matrix can be split into three matrices.
mat = rot * trans * corr, where rot is rotation part, trans is translation part, and corr is the correction due to perspective (if any).
rot = [ r00 r01 r02 0
r10 r11 r12 0
r20 r21 r22 0
0   0   0   1 ]
trans = [ 1  0  0  0
0  1  0  0
0  0  1  0
tx ty tz 1 ]
corr = [ 1 0 0 px
0 1 0 py
0 0 1 pz
0 0 0 s ]
where the elements of corr are obtained from linear combinations of the elements of rot, trans, and mat.
So the inverse is mat' = (trans * corr)' * rot', where rot' must be computed the traditional way, which is easy since it is only a 3x3 matrix.
This problem is simplified if [px py pz s] = [0 0 0 1], which will happen if mat was composed only of rotations, scales, and translations (which is common).  In this case, we can ignore corr entirely which saves on a lot of computations.
******************************************/

template <class T>
inline T SGL_ABS(T a)
{
	return (a >= 0 ? a : -a);
}

#ifndef SGL_SWAP
#define SGL_SWAP(a,b,temp) ((temp)=(a),(a)=(b),(b)=(temp))
#endif

bool CTransform::invert( const CTransform& mat )
{
	if (&mat==this) 
	{
		CTransform tm(mat);
		return invert(tm);
	}

	unsigned int indxc[4], indxr[4], ipiv[4];
	unsigned int i,j,k,l,ll;
	unsigned int icol = 0;
	unsigned int irow = 0;
	double temp, pivinv, dum, big;

	// copy in place this may be unnecessary
	*this = mat;

	for (j=0; j<4; j++) ipiv[j]=0;

	for(i=0;i<4;i++)
	{
		big=0.0;
		for (j=0; j<4; j++)
			if (ipiv[j] != 1)
				for (k=0; k<4; k++)
				{
					if (ipiv[k] == 0)
					{
						if (SGL_ABS(operator()(j,k)) >= big)
						{
							big = SGL_ABS(operator()(j,k));
							irow=j;
							icol=k;
						}
					}
					else if (ipiv[k] > 1)
						return false;
				}
				++(ipiv[icol]);
				if (irow != icol)
					for (l=0; l<4; l++) SGL_SWAP(operator()(irow,l),
						operator()(icol,l),
						temp);

				indxr[i]=irow;
				indxc[i]=icol;
				if (operator()(icol,icol) == 0)
					return false;

				pivinv = 1.0/operator()(icol,icol);
				operator()(icol,icol) = 1;
				for (l=0; l<4; l++) operator()(icol,l) *= pivinv;
				for (ll=0; ll<4; ll++)
					if (ll != icol)
					{
						dum=operator()(ll,icol);
						operator()(ll,icol) = 0;
						for (l=0; l<4; l++) operator()(ll,l) -= operator()(icol,l)*dum;
					}
	}
	for (int lx=4; lx>0; --lx)
	{
		if (indxr[lx-1] != indxc[lx-1])
			for (k=0; k<4; k++) SGL_SWAP(operator()(k,indxr[lx-1]),
				operator()(k,indxc[lx-1]),temp);
	}

	return true;
}

#undef SET_ROW

CTransform CTransform::identity(void)
{
	CTransform m;
	m.makeIdentity();
	return m;
}

CTransform CTransform::scale(double sx, double sy, double sz)
{
	CTransform m;
	m.makeScale(sx,sy,sz);
	return m;
}

CTransform CTransform::scale(const CVec & v )
{
	return scale(v.GetVecX(), v.GetVecY(), v.GetVecZ() );
}

CTransform CTransform::translate(double tx, double ty, double tz)
{
	CTransform m;
	m.makeTranslate(tx,ty,tz);
	return m;
}

CTransform CTransform::translate(const CVec & v )
{
	return translate(v.GetVecX(), v.GetVecY(), v.GetVecZ());
}

CTransform CTransform::rotate(double angle, double x, double y, double z )
{
	CTransform m;
	m.makeRotate(angle,x,y,z);
	return m;
}

CTransform CTransform::rotate(double angle, const CVec & axis )
{
	CTransform m;
	m.makeRotate(angle,axis);
	return m;
}

CTransform CTransform::rotate(double angle, const CPos & pos, const CVec & axis )
{
	CTransform m1;
	m1.makeTranslate(-pos.GetPosX(), -pos.GetPosY(), -pos.GetPosZ());
	CTransform m2;
	m2.makeRotate(angle,axis);
	CTransform m3;
	m3.makeTranslate(pos.GetPosX(), pos.GetPosY(), pos.GetPosZ());

	CTransform m=m1*m2*m3;

	return m;
}

CTransform CTransform::rotate(const CVec & from, const CVec & to, const CVec *pRefVec)
{
	CTransform m;
	m.makeRotate(from,to, pRefVec);
	return m;
}

CTransform CTransform::inverse( const CTransform& matrix)
{
	CTransform m;
	m.invert(matrix);
	return m;
}

CTransform CTransform::CoordSystem(const CPos & Origin, const CVec & ZAxis, const CVec & XAxis)
{
	CTransform translate=CTransform::translate(Origin.GetPosX(), Origin.GetPosY(), Origin.GetPosZ());
	CTransform rotate1=CTransform::rotate(CVec(0, 0, 1), ZAxis);
	CVec xaxis_adjusted=rotate1.TransformVec(CVec(1, 0, 0));
	CTransform rotate2=CTransform::rotate(xaxis_adjusted, XAxis, &ZAxis);

	return rotate1*rotate2*translate;
}

CTransform CTransform::MirrorByPlane(double nx, double ny, double nz)
{
	CTransform m;
	m.makeMirrorByPlane(nx, ny, nz);
	return m;
}

CTransform CTransform::MirrorByPlane(const CVec & vec)
{
	CTransform m;
	m.makeMirrorByPlane(vec.GetVecX(), vec.GetVecY(), vec.GetVecZ());
	return m;
}

CTransform CTransform::MirrorByPlane(const CPos & pos, const CVec & vec)
{
	CTransform m1;
	m1.makeTranslate(-pos.GetPosX(), -pos.GetPosY(), -pos.GetPosZ());
	CTransform m2;
	m2.makeMirrorByPlane(vec.GetVecX(), vec.GetVecY(), vec.GetVecZ());
	CTransform m3;
	m3.makeTranslate(pos.GetPosX(), pos.GetPosY(), pos.GetPosZ());

	CTransform m=m1*m2*m3;

	return m;
}

CTransform CTransform::MirrorByPoint(double pt_x, double pt_y, double pt_z)
{
	CTransform m1;
	m1.makeTranslate(-pt_x, -pt_y, -pt_z);
	CTransform m2;
	m2.makeScale(-1, -1, -1);
	CTransform m3;
	m3.makeTranslate(pt_x, pt_y, pt_z);

	CTransform m=m1*m2*m3;

	return m;
}

CTransform CTransform::MirrorByPoint(const CPos & pos)
{
	CTransform m1;
	m1.makeTranslate(-pos.GetPosX(), -pos.GetPosY(), -pos.GetPosZ());
	CTransform m2;
	m2.makeScale(-1, -1, -1);
	CTransform m3;
	m3.makeTranslate(pos.GetPosX(), pos.GetPosY(), pos.GetPosZ());

	CTransform m=m1*m2*m3;

	return m;
}

CPos CTransform::TransformPos(const CPos & p) const
{
	return CPos( 
		(m_Data[0][0]*p.GetPosX() + m_Data[1][0]*p.GetPosY() + m_Data[2][0]*p.GetPosZ()+m_Data[3][0]),
		(m_Data[0][1]*p.GetPosX() + m_Data[1][1]*p.GetPosY() + m_Data[2][1]*p.GetPosZ()+m_Data[3][1]),
		(m_Data[0][2]*p.GetPosX() + m_Data[1][2]*p.GetPosY() + m_Data[2][2]*p.GetPosZ()+m_Data[3][2]) ) ;
	//return CPos( (m_Data[0][0]*p.GetPosX() + m_Data[0][1]*p.GetPosY() + m_Data[0][2]*p.GetPosZ()),
	//	(m_Data[1][0]*p.GetPosX() + m_Data[1][1]*p.GetPosY() + m_Data[1][2]*p.GetPosZ()),
	//	(m_Data[2][0]*p.GetPosX() + m_Data[2][1]*p.GetPosY() + m_Data[2][2]*p.GetPosZ()) ) ;
}

CVec CTransform::TransformVec(const CVec & vec) const
{
	CPos pos(vec.GetVecX(), vec.GetVecY(), vec.GetVecZ());
	CPos pos_base(0, 0, 0);

	CPos pos_=TransformPos(pos);
	CPos pos_base_=TransformPos(pos_base);

	return CVec(pos_.GetPosX()-pos_base_.GetPosX(), pos_.GetPosY()-pos_base_.GetPosY(), pos_.GetPosZ()-pos_base_.GetPosZ());
}